/**
 * @file longitudinal_control
 * @author yq
 * @brief 
 * @version 0.1
 * @date 24-3-18
 *
 * @copyright Copyright (c) 2024
 */
#include "longitudinal_control.h"
#include <ros/ros.h>

PidConf::PidConf(double kp, double ki, double kd, double target, double windup_guard, double low_bound,
                 double up_bound) : kp(kp), ki(ki), kd(kd), target(target), windup_guard(windup_guard),
                                    low_bound(low_bound), up_bound(up_bound), error(0.0), pre_error(0.0),
                                    derivative_error(0.0), integral_error(0.0), real_ratio(0.0){}

void PidConf::SetKp(double p) { this->kp = p; }
void PidConf::SetKi(double i) { this->ki = i; }
void PidConf::SetKd(double d) { this->kd = d; }
void PidConf::SetTarget(double target) { this->target = target; }
void PidConf::SetRealRatio(double ratio) { this->real_ratio = ratio; }

Pid::Pid(PidConf pidconf) : pidconf(pidconf) {}

double Pid::GetTarget() const { return pidconf.target; }
double Pid::GetRealRatio() const { return pidconf.real_ratio; }
void Pid::SetRealRatio(double real_ratio) { pidconf.SetRealRatio(real_ratio); }
void Pid::SetTarget(double target) { pidconf.SetTarget(target); }

double Pid::Output() {
    // Calculate error
    pidconf.error = pidconf.target - pidconf.real_ratio;
    // Proportional term
    double Pout = pidconf.kp * pidconf.error;
    // Integral term
//    pidconf.integral_error += pidconf.error;
    if (fabs(pidconf.error) < 0.1) {
        pidconf.integral_error = 0;
    }
    pidconf.integral_error = std::clamp(pidconf.integral_error, -pidconf.windup_guard, pidconf.windup_guard);
    double Iout = pidconf.ki * pidconf.integral_error;
    // Derivative term
    pidconf.derivative_error = pidconf.error - pidconf.pre_error;
    double Dout = pidconf.kd * pidconf.derivative_error;
    // Save error to previous error
    pidconf.pre_error = pidconf.error;
    // Calculate total output
    ROS_INFO("pidconf.error: %f", pidconf.error);
    ROS_INFO("Pout: %f, Iout: %f, Dout: %f", Pout, Iout, Dout);
    return Pout + Iout + Dout;
}
